#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 调用 transform 必须包含该头文件
/*
    需求：订阅方：订阅发布的坐标系相对关系，传入一个 坐标点，调用 tf 实现转换
    流程：
        1、包含头文件 
        2、编码，初始化， NodeHandle (必须 的)
        3、创建订阅对象--->订阅坐标系相对关系 
        4、组织一个 坐标点数据 
        5、转换算法，需要调用 TF  内置实现
        6、最后输出

*/

int main(int argc, char *argv[])
{
    // 2、编码，初始化， NodeHandle (必须 的)
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle nh; 

    // 3、创建订阅对象--->订阅坐标系相对关系 
    // 3-1、创建一个  buffer 缓存
    tf2_ros::Buffer buffer;

    // 3-2、创建监听对象（监听对象会把订阅的数据存入buffer ）
    // 这里的 buffer 和 TransformListener 是结合使用的，只要使用就必须有这两个一起
    tf2_ros::TransformListener  listener(buffer);
    
    // 4、组织一个 坐标点数据 
    geometry_msgs::PointStamped ps; 
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    // 这里的坐标点虽然是写死的，实际业务中，是动态生成的
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;

    // 方案1：在调用转换函数前，执行休眠 添加休眠
    // ros::Duration(2).sleep();

    // 5、转换算法，需要调用 TF  内置实现
    // 设置转换频率，每秒10次
    ros::Rate rate(10);

    while(ros::ok()) 
    {
        // 核心代码  ----将 ps转换成相对于 base_link 的坐标点
        geometry_msgs::PointStamped ps_out;


        /*   如果不添加上面的休眠，就会报如下错误，因为没有保证节点注册后能拿到节点对应关系 
            休眠2秒后，就能保存 节点已经在master中注册完成，获取到节点间相对关系 
        xhb@xhb-linux-2:~/ros/catkin_ws$ rosrun tf1_static demo02_static_sub 
        terminate called after throwing an instance of 'tf2::LookupException'
        what():  "base_link" passed to lookupTransform argument target_frame does not exist. 
        已放弃 (核心已转储)
        */

        /*
        调用了 buffer 的转换函数 transform 
        参数1： 被转换的坐标点
        参数2：目标坐标系
        返回值：转出的坐标点

        PS1 : 调用时必须包含头文件  tf2_geometry_msgs/tf2_geometry_msgs.h 
        PS2：运行时存在的问题输出一个 异常 base_link 不存在 
                原因：订阅数据是一个 耗时操作，可能在调用  transform 转换函数时，坐标系的相对
                            关系还没有 订阅到，因此出现异常
                解决：
                    方案1：在调用转换函数前，执行休眠 
                    方案2：进行异常处理
         */

        try 
        {
            ps_out = buffer.transform(ps, "base_link");

            // 6、最后输出
            ROS_INFO("转换后的坐标值：（%.2f, %.2f, %.2f）, 参考的坐标系：%s", 
                                    ps_out.point.x,
                                    ps_out.point.y,
                                    ps_out.point.z,
                                    ps_out.header.frame_id.c_str()
                                    );
        } 
        catch(const std::exception& e) 
        {
            std::cerr << e.what() << '\n';
            // ROS_INFO("程序异常。。。。。");
        }
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}
